#include <stdint.h>
#include <string.h>
#include "rm12xx_flash.h"

void delay(unsigned int i) {
	while (i--) ;
}

int flash_physical_write(uint32_t addr, uint32_t size, const uint8_t *data) {
	int status = RM12XX_FLASH_STATUS_OK;
    if( ((addr >= CACHE_BASE_ADR) && (addr <= CACHE_END_ADR)) || 
    ((addr >= FLASH_UNCACHED_BASE) && (addr <= FLASH_UNCACHED_END))) {
       status = drv_flash_write(addr, data, size);
       if (status != RM12XX_FLASH_STATUS_OK) {
           return status;
       }
        delay(100);
    } else {
       //for(uint32_t i=0; i<count; i++) {
       //    *((uint8_t*)address+i) = buffer[i];
       //}
    }
	return status;
}

int flash_physical_erase(uint32_t addr, uint32_t size) {
	uint32_t command;
	/* Alignment has been checked in upper layer */
	for (; size > 0; size -= RM12XX_FLASH_ERASE_SIZE,
		addr += RM12XX_FLASH_ERASE_SIZE) {
		/* Start erase */
		command = FLASH_ERASE_SECTOR;
        if((addr >= CACHE_BASE_ADR) && (addr <= CACHE_END_ADR)) {
            addr -=  CACHE_BASE_ADR;
            drv_flash_unit_erase(command, addr);
        } else if((addr >= FLASH_UNCACHED_BASE) && (addr <= FLASH_UNCACHED_END)) {
            addr -= FLASH_UNCACHED_BASE;
            drv_flash_unit_erase(command, addr);
        }
	}
	return RM12XX_FLASH_STATUS_OK;
}

int flash_physical_erase_all(void) {
    uint32_t command = FLASH_ERASE_ALL_CHIP;
    drv_flash_unit_erase(command, 0);

	return RM12XX_FLASH_STATUS_OK;
}
int flash_get_id(uint32_t *id) {

	return RM12XX_FLASH_STATUS_OK;
}


/*----------------------------------------------------------------------------
 *                             flash loader function
 *----------------------------------------------------------------------------*/
uint32_t flashloader_init(struct rm12xx_flash_params *params)
{
	/* Initialize params buffers */
	memset(params, 0, sizeof(struct rm12xx_flash_params));

	return RM12XX_FLASH_STATUS_OK;
}

/*----------------------------------------------------------------------------
 *                                      Functions
 *----------------------------------------------------------------------------*/
/* flashloader parameter structure */
__attribute__ ((section(".buffers.g_cfg")))
volatile struct rm12xx_flash_params g_cfg;
/* data buffer */
__attribute__ ((section(".buffers.g_buf")))
uint8_t g_buf[RM12XX_FLASH_LOADER_BUFFER_SIZE];

int main(void)
{
	uint32_t id = 0;
    int status  = 0;

	/* set buffer */
	flashloader_init((struct rm12xx_flash_params *)&g_cfg);

	/* clear flash status registers 
	int status = flash_physical_clear_stsreg();
	if (status != RM12XX_FLASH_STATUS_OK) {
		while (1)
			g_cfg.sync = status;
	}*/

	while (1) {
		/* wait command*/
		while (g_cfg.sync == RM12XX_FLASH_LOADER_WAIT)
			;

		/* command handler */
		switch (g_cfg.cmd) {
		case RM12XX_FLASH_CMD_GET_FLASH_ID:
			status = flash_get_id(&id);
			if (status == RM12XX_FLASH_STATUS_OK) {
				g_buf[0] = id & 0xff;
				g_buf[1] = (id >> 8) & 0xff;
				g_buf[2] = (id >> 16) & 0xff;
				g_buf[3] = 0x00;
			}
			break;
		case RM12XX_FLASH_CMD_ERASE_SECTORS:
			status = flash_physical_erase(g_cfg.addr, g_cfg.len);
			break;
		case RM12XX_FLASH_CMD_ERASE_ALL:
			status = flash_physical_erase_all();
			break;
		case RM12XX_FLASH_CMD_PROGRAM:
			status = flash_physical_write(g_cfg.addr,
							g_cfg.len,
							g_buf);
			break;
		default:
			status = RM12XX_FLASH_STATUS_FAILED_UNKNOWN_COMMAND;
			break;
		}

		/* clear & set result for next command */
		if (status != RM12XX_FLASH_STATUS_OK) {
			g_cfg.sync = status;
			while (1)
				;
		} else {
			g_cfg.sync = RM12XX_FLASH_LOADER_WAIT;
		}
	}

	return 0;
}

__attribute__ ((section(".stack")))
__attribute__ ((used))
static uint32_t stack[RM12XX_FLASH_LOADER_STACK_SIZE / 4];
extern uint32_t _estack;
extern uint32_t _bss;
extern uint32_t _ebss;

__attribute__ ((section(".entry")))
__attribute__((naked)) __attribute__((noreturn)) void entry(void)
{
	//__asm(" bkpt #0x00");
	/* set sp from end of stack */
	//__asm(" ldr r0, =_estack - 4");
	//__asm(" mov sp, r0");

	main();

	__asm(" bkpt #0x00");
}
